#!/usr/bin/env python3
import numpy as np
import transforms3d as t3d

def calculate_correct_matrices():
    print("=== 计算正确的Piper变换矩阵 ===")
    
    # Piper URDF中的变换
    joint6_rpy = [-1.5708, -3.1415926, 0]  # joint6 origin
    visual_rpy = [0, 0, 1.5708]  # link6 visual origin
    
    # 计算总变换
    joint6_mat = t3d.euler.euler2mat(*joint6_rpy)
    visual_mat = t3d.euler.euler2mat(*visual_rpy)
    total_mat = joint6_mat @ visual_mat
    
    print(f"Joint6 变换矩阵:\n{joint6_mat}")
    print(f"Visual 变换矩阵:\n{visual_mat}")
    print(f"总变换矩阵:\n{total_mat}")
    
    # 我们希望最终结果接近单位矩阵，所以需要补偿这个变换
    # 如果我们要让最终结果 = total_mat @ global_trans @ delta = I
    # 那么 global_trans @ delta = total_mat^(-1)
    
    total_inv = np.linalg.inv(total_mat)
    print(f"总变换的逆矩阵:\n{total_inv}")
    
    # 分解为两个矩阵的乘积
    # 我们可以选择 delta = I, global_trans = total_inv
    # 或者 delta = total_inv, global_trans = I
    # 或者分解为其他形式
    
    # 方案1: delta = I, global_trans = total_inv
    delta1 = np.eye(3)
    global1 = total_inv
    
    print(f"\n方案1:")
    print(f"delta_matrix: {delta1.tolist()}")
    print(f"global_trans_matrix: {global1.tolist()}")
    print(f"验证: delta @ global = {delta1 @ global1}")
    
    # 方案2: 分解为更简单的形式
    # 尝试分解为旋转矩阵的乘积
    from scipy.linalg import polar
    R, P = polar(total_inv)
    
    print(f"\n方案2 (极分解):")
    print(f"旋转部分 R:\n{R}")
    print(f"验证 R是旋转矩阵: det(R)={np.linalg.det(R)}")
    
    # 方案3: 直接使用URDF变换的逆
    delta3 = np.eye(3)
    global3 = total_inv
    
    print(f"\n方案3 (直接使用逆变换):")
    print(f"delta_matrix: {delta3.tolist()}")
    print(f"global_trans_matrix: {global3.tolist()}")
    
    # 测试这个配置
    test_quat = t3d.quaternions.mat2quat(total_mat)
    final_mat = total_mat @ global3 @ delta3
    final_quat = t3d.quaternions.mat2quat(final_mat)
    
    print(f"\n测试:")
    print(f"原始四元数: {test_quat}")
    print(f"最终四元数: {final_quat}")
    print(f"与单位四元数差异: {final_quat - np.array([1,0,0,0])}")

if __name__ == "__main__":
    calculate_correct_matrices()
